#include <glog/logging.h>
#include "quaternion_kinematics.h"

namespace sad {

QuaternionKinematics::QuaternionKinematics() {
    options_ = Options();
    BuildNoise(options_); 
    memset(&last_odom_, 0.0, sizeof(Odom));

    int_method_ = IntegralMethod::MEDIAN_VALUE;
}

bool QuaternionKinematics::Predict(const IMU& imu) {
    assert(imu.timestamp_ >= current_time_);

    double dt = imu.timestamp_ - current_time_;
    
    //LOG(INFO) << "this imu dt_ = " << dt;

    if (dt > (5 * options_.imu_dt_) || dt < 0) {
        // 时间间隔不对，可能是第一个IMU数据，没有历史信息
        LOG(INFO) << "skip this imu because dt_ = " << dt;
        current_time_ = imu.timestamp_;
        last_imu_ = imu;
        return false;
    }

    double dt2 = dt * dt;
    
    Eigen::Vector3d delta_angle_axis;
    Eigen::Vector3d acc_unbias;
    Eigen::Vector3d gyro_unbias;
    if(int_method_ == IntegralMethod::MEDIAN_VALUE) {
        // Acc and gyro.
        acc_unbias = 0.5 * (last_imu_.acce_ + imu.acce_) - ba_;
        gyro_unbias = 0.5 * (last_imu_.gyro_ + imu.gyro_) - bg_;
    
        delta_angle_axis = gyro_unbias * dt;
        p_ += v_ * dt + 0.5 * (rot_ * acc_unbias + g_) * dt2;
        v_ += (rot_ * acc_unbias + g_) * dt;  
    } else if(int_method_ == IntegralMethod::MEAN_VALUE) {
        acc_unbias = imu.acce_ - ba_;
        gyro_unbias = imu.gyro_ - bg_;
        // nominal state 递推
        Eigen::Vector3d delta_angle_axis = gyro_unbias * dt;
        p_ = p_ + v_ * dt + 0.5 * (rot_ * acc_unbias) * dt * dt + 0.5 * g_ * dt * dt;
        v_ = v_ + rot_ * acc_unbias * dt + g_ * dt;
    } else {
        // TODO: Runge-Kutta
    }

    Eigen::Matrix3d delta_rot = Eigen::Matrix3d::Identity();

    if (delta_angle_axis.norm() > 1e-15) {
        delta_rot = Eigen::AngleAxisd(delta_angle_axis.norm(), delta_angle_axis.normalized()).toRotationMatrix();
    }
    rot_ = rot_ * delta_rot;

    // 其余状态维度不变
    // error state 递推
    // 计算运动过程雅可比矩阵 F，见(3.47)
    // F实际上是稀疏矩阵，也可以不用矩阵形式进行相乘而是写成散装形式，这里为了教学方便，使用矩阵形式
    // dp, dv, dR, dbg, dba, dg
    Mat18T F = Mat18T::Identity();                                                 // 主对角线
    F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;                         // p 对 v
    F.template block<3, 3>(3, 6) = -rot_ * SkewMatrix(acc_unbias) * dt;       // v对theta
    F.template block<3, 3>(3, 12) = -rot_ * dt;                             // v 对 ba
    F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;                        // v 对 g
    F.template block<3, 3>(6, 6) = delta_rot.transpose();  
    F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;                        // theta 对 bg

    // mean and cov prediction
    dx_ = F * dx_;  // 这行其实没必要算，dx_在重置之后应该为零，因此这步可以跳过，但F需要参与Cov部分计算，所以保留
    
    //LOG(INFO) << "delta_angle_axis = " << delta_angle_axis.transpose();
    //LOG(INFO) << "dx_.block<3, 1>(6, 0) = " << dx_.block<3, 1>(6, 0).transpose();
    

    Eigen::Matrix<double, 18, 12> Fi = Eigen::Matrix<double, 18, 12>::Zero();
    Fi.block<12, 12>(3, 0) = Eigen::Matrix<double, 12, 12>::Identity();

    Eigen::Matrix<double, 12, 12> Qi = Eigen::Matrix<double, 12, 12>::Zero();
    Qi.block<3, 3>(0, 0) = options_.acce_var_ * Eigen::Matrix3d::Identity();
    Qi.block<3, 3>(3, 3) = options_.gyro_var_ * Eigen::Matrix3d::Identity();
    Qi.block<3, 3>(6, 6) = options_.bias_acce_var_ * Eigen::Matrix3d::Identity();
    Qi.block<3, 3>(9, 9) = options_.bias_gyro_var_ * Eigen::Matrix3d::Identity();
    
    Q_ = Fi * Qi * Fi.transpose();

    cov_ = F * cov_.eval() * F.transpose() + Q_;
    current_time_ = imu.timestamp_;
    last_imu_ = imu;
    return true;
}

bool QuaternionKinematics::ObserveWheelPoseSpeed(const Odom& odom, const NavStateT& last_odom_state) {
    assert(odom.timestamp_ >= current_time_);

    options_.odom_span_ = odom.timestamp_ - last_odom_.timestamp_;
    if(options_.odom_span_ > 1.0 || options_.odom_span_ < 1.0e-5) {
        options_.odom_span_ = 0.01;
    }     
    double velo_l = options_.left_wheel_radius_ * (odom.left_pulse_) / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
    double velo_r = options_.right_wheel_radius_ * (odom.right_pulse_) / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
  
    double average_vel = 0.5 * (velo_l + velo_r);

    VecT vel_odom(average_vel, 0.0, 0.0);
    
    // Pre-computation.
    const double left_dist = odom.left_pulse_ * 0.00047820240382508;
    const double right_dist = odom.right_pulse_ * 0.00047768621928995;
    const double delta_yaw = (right_dist - left_dist) / 1.52439;
    const double delta_dist = (right_dist + left_dist) * 0.5;
    // Mean.
    const Eigen::Matrix3d delta_rot = Eigen::AngleAxisd(delta_yaw, Eigen::Vector3d::UnitZ()).toRotationMatrix();
    const Eigen::Vector3d delta_trans = Eigen::Vector3d(delta_dist, 0.0, 0.0);
    Eigen::Matrix4d delta_odom = Eigen::Matrix4d::Identity();
    delta_odom.block<3,3>(0,0) = delta_rot;
    delta_odom.block<3,1>(0,3) = delta_trans;

    Eigen::Vector3d vel_world = rot_ * vel_odom;
    
    Eigen::Matrix4d last_state_pose = Eigen::Matrix4d::Identity();
    //last_state_pose.block<3,3>(0,0) = last_odom_state.R_.matrix(); 
    //last_state_pose.block<3,1>(0,3) = last_odom_state.p_;
    last_state_pose.block<3,3>(0,0) = last_rot_; 
    last_state_pose.block<3,1>(0,3) = last_p_;

    Eigen::Matrix4d predict_odom_pose = last_state_pose * delta_odom; 
    
    //std::cout << "vel_world= " << vel_world << std::endl;

    //std::cout << "delta_odom= " << delta_odom << std::endl;

    //std::cout << "last_state_pose= " << last_state_pose << std::endl;
    //std::cout << "predict_odom_pose= " << predict_odom_pose << std::endl;
    
    Eigen::Matrix<double, 3, 3> wheel_rot = predict_odom_pose.block<3,3>(0,0);
    Eigen::Vector3d wheel_trans = predict_odom_pose.block<3,1>(0,3);
    Eigen::Matrix<double, 9, 18> H = Eigen::Matrix<double, 9, 18>::Zero();
    H.template block<3, 3>(0, 0) = Eigen::Matrix<double, 3, 3>::Identity();
    H.template block<3, 3>(3, 3) = Eigen::Matrix<double, 3, 3>::Identity();
    H.template block<3, 3>(6, 6) = Eigen::Matrix<double, 3, 3>::Identity();

    double trans_noise = 0.01;
    double ang_noise = 0.5;
    double speed_noise = 0.01;
    if(fabs(average_vel) < 0.001) {
        trans_noise *= trans_noise;
        ang_noise *= ang_noise;
        speed_noise *= speed_noise;
    } 

    Eigen::Matrix<double, 9, 1> noise_vec;
    
    noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise, speed_noise, speed_noise, speed_noise;
    Eigen::Matrix<double, 9, 9> V = noise_vec.asDiagonal();
    Eigen::Matrix<double, 18, 9> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + V).inverse();
    
    //std::cout << "cov_= " << cov_ << std::endl;
    //std::cout << "K= " << K << std::endl;

    Eigen::Matrix<double, 9, 1> innovation = Eigen::Matrix<double, 9, 1>::Zero();
    innovation.template head<3>() = wheel_trans - p_;  
    innovation.block<3, 1>(3, 0) =  vel_world - v_;       
    innovation.template tail<3>() = Log((rot_.inverse() * wheel_rot));
    
    dx_ = K * innovation;
    cov_ = (Eigen::Matrix<double, 18, 18>::Identity() - K * H) * cov_;
    
    //std::cout << "dx_= " << dx_ << std::endl;

    UpdateAndReset();

    last_odom_ = odom;
    last_p_ = p_;
    last_rot_ = rot_;

    //std::cout << "p_= " << p_ << std::endl;
    //std::cout << "v_= " << v_ << std::endl;
    //std::cout << "rot_= " << rot_ << std::endl;     
    //std::cout << "innovation= " << innovation.transpose() << std::endl;    

    return true;
}

bool QuaternionKinematics::ObserveGps(const GNSS& gnss) {
    //LOG(INFO) << "gnss.unix_time_ = " << gnss.unix_time_;

    /// GNSS 观测的修正
    assert(gnss.unix_time_ >= current_time_);
    //LOG(INFO) << "first_gnss_ = " << first_gnss_;
    gnss_ = gnss;

    if (first_gnss_) {
        rot_ = gnss.utm_pose_.so3().matrix();
        p_ = gnss.utm_pose_.translation();
        first_gnss_ = false;
        current_time_ = gnss.unix_time_;
        return true;
    }

    assert(gnss.heading_valid_);
    //LOG(INFO) << "gnss.heading_valid_ = " << gnss.heading_valid_;
    ObserveSE3(gnss.utm_pose_, options_.gnss_pos_noise_, options_.gnss_ang_noise_);
    //ObservePose(gnss.utm_pose_, options_.gnss_pos_noise_, options_.gnss_ang_noise_);
    //ObserveQuaternion(gnss.utm_pose_, options_.gnss_pos_noise_, options_.gnss_ang_noise_);
    //ObserveRotation(gnss.utm_pose_, options_.gnss_pos_noise_, options_.gnss_ang_noise_);
    current_time_ = gnss.unix_time_;
}

bool QuaternionKinematics::ObserveSE3(const SE3& pose, double trans_noise, double ang_noise) {
    Eigen::Matrix3d Rgnss = pose.so3().matrix();

    /// 既有旋转，也有平移
    /// 观测状态变量中的p, R，H为6x18，其余为零
    Eigen::Matrix<double, 6, 18> H = Eigen::Matrix<double, 6, 18>::Zero();
    H.template block<3, 3>(0, 0) = Mat3T::Identity();  // P部分
    H.template block<3, 3>(3, 6) = Mat3T::Identity();  // R部分（3.66)
    
    Vec3d lat_lon_alt_std(gnss_.lat_lon_alt_std_[0], gnss_.lat_lon_alt_std_[1], gnss_.lat_lon_alt_std_[2]);

    // 卡尔曼增益和更新过程
    Vec6d noise_vec;
    if(is_use_wheel_se3_) {
        noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;
    } else {
        noise_vec << lat_lon_alt_std[0], lat_lon_alt_std[1], lat_lon_alt_std[2], ang_noise, ang_noise, ang_noise;
    }

    Mat6d V = noise_vec.asDiagonal();
    Eigen::Matrix<double, 18, 6> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + V).inverse();

    // 更新x和cov
    Vec6d innov = Vec6d::Zero();
    innov.template head<3>() = (pose.translation() - p_);          // 平移部分
    innov.template tail<3>() = Log((rot_.inverse() * Rgnss));  // 旋转部分(3.67)
    
    dx_ = K * innov;
    cov_ = (Mat18T::Identity() - K * H) * cov_;

    UpdateAndReset();
    return true;
}

bool QuaternionKinematics::ObservePose(const SE3& pose, double trans_noise, double ang_noise) {
    Eigen::Matrix3d Rgnss = pose.so3().matrix();
    Eigen::Matrix<double, 3, 1> t_gnss = pose.translation();

    /// 既有旋转，也有平移
    /// 观测状态变量中的p, R，H为6x18，其余为零
    Eigen::Matrix<double, 3, 18> H = Eigen::Matrix<double, 3, 18>::Zero();
    H.template block<3, 3>(0, 0) = Mat3T::Identity();  // P部分
    
    Vec3d lat_lon_alt_std(gnss_.lat_lon_alt_std_[0], gnss_.lat_lon_alt_std_[1], gnss_.lat_lon_alt_std_[2]);

    // 卡尔曼增益和更新过程
    Vec3d noise_vec;
    noise_vec << lat_lon_alt_std[0], lat_lon_alt_std[1], lat_lon_alt_std[2];

    Mat3d V = noise_vec.asDiagonal();
    Eigen::Matrix<double, 18, 3> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + V).inverse();

    // 更新x和cov
    Vec3d innov = Vec3d::Zero();
    innov.template head<3>() = (t_gnss - p_);          // 平移部分
    dx_ = K * innov;
    cov_ = (Mat18T::Identity() - K * H) * cov_;

    UpdateAndReset();
    return true;
}
// Note: bad result
bool QuaternionKinematics::ObserveQuaternion(const SE3& pose, double trans_noise, double ang_noise) {
    Eigen::Matrix3d Rgnss = pose.so3().matrix();
    Eigen::Matrix<double, 3, 1> t_gnss = pose.translation();

    Eigen::Matrix<double, 4, 3> Qtheta;

    quat_ = Eigen::Quaterniond(rot_);
    quat_.normalized();

    Qtheta(0,0) = -quat_.x();
    Qtheta(0,1) = -quat_.y();
    Qtheta(0,2) = -quat_.z();
    Qtheta(1,0) = quat_.w();
    Qtheta(1,1) = -quat_.z();
    Qtheta(1,2) = quat_.y();
    Qtheta(2,0) = quat_.z();
    Qtheta(2,1) = quat_.w();
    Qtheta(2,2) = -quat_.x();
    Qtheta(3,0) = -quat_.y();
    Qtheta(3,1) = quat_.x();
    Qtheta(3,2) = quat_.w();

    /*Eigen::Vector3d delta_q = dx_.block<3, 1>(6, 0);
    Eigen::Quaterniond temp_quat_trans;

    temp_quat_trans.w() = 1;
    temp_quat_trans.x() = delta_q[0]/2;
    temp_quat_trans.y() = delta_q[1]/2;
    temp_quat_trans.z() = delta_q[2]/2;
    Eigen::Matrix<double, 4, 4> right_quat = ComputeRightQuaternion(temp_quat_trans);
    Qtheta = right_quat*Qtheta;*/

    Qtheta *= 0.5;

    /// 既有旋转，也有平移
    /// 观测状态变量中的p, R，H为6x18，其余为零
    Eigen::Matrix<double, 7, 18> H = Eigen::Matrix<double, 7, 18>::Zero();
    H.template block<3, 3>(0, 0) = Mat3T::Identity();  // P部分
    H.template block<4, 3>(3, 6) = Qtheta;  // R部分（3.66)
    
    Vec3d lat_lon_alt_std(gnss_.lat_lon_alt_std_[0], gnss_.lat_lon_alt_std_[1], gnss_.lat_lon_alt_std_[2]);

    // 卡尔曼增益和更新过程
    Eigen::Matrix<double, 7, 1> noise_vec;
    noise_vec << lat_lon_alt_std[0], lat_lon_alt_std[1], lat_lon_alt_std[2], ang_noise, ang_noise, ang_noise, ang_noise;

    Eigen::Matrix<double, 7, 7> V = noise_vec.asDiagonal();
    Eigen::Matrix<double, 18, 7> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + V).inverse();

    Eigen::Quaterniond temp_quat = quat_ * Eigen::Quaterniond(Rgnss).conjugate();

    //temp_quat.normalized();

    Eigen::Matrix<double, 4, 1> quat;
    quat(0, 0) = temp_quat.w();
    quat(1, 0) = temp_quat.x();
    quat(2, 0) = temp_quat.y();
    quat(3, 0) = temp_quat.z();  

    LOG(INFO) << "quat = " << quat.transpose();

    // 更新x和cov
    Eigen::Matrix<double, 7, 1> innov = Eigen::Matrix<double, 7, 1>::Zero();
    innov.template head<3>() = (t_gnss - p_);          // 平移部分
    innov.template tail<4>() = quat;  // 旋转部分(3.67)
    
    dx_ = K * innov;
    cov_ = (Mat18T::Identity() - K * H) * cov_;

    UpdateQuaternionAndReset();
    return true;
}

bool QuaternionKinematics::ObserveRotation(const SE3& pose, double trans_noise, double ang_noise) {
    Eigen::Matrix3d Rgnss = pose.so3().matrix();
    Eigen::Matrix<double, 3, 1> t_gnss = pose.translation();

    /// 既有旋转，也有平移
    /// 观测状态变量中的p, R，H为6x18，其余为零
    Eigen::Matrix<double, 6, 18> H = Eigen::Matrix<double, 6, 18>::Zero();
    H.template block<3, 3>(0, 0) = Mat3T::Identity();  // P部分
    H.template block<3, 3>(3, 6) = ComputeRotationJacbian();  // R部分（3.66)
    
    Vec3d lat_lon_alt_std(gnss_.lat_lon_alt_std_[0], gnss_.lat_lon_alt_std_[1], gnss_.lat_lon_alt_std_[2]);

    // 卡尔曼增益和更新过程
    Vec6d noise_vec;
    if(is_use_wheel_se3_) {
        noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;
    } else {
        noise_vec << lat_lon_alt_std[0], lat_lon_alt_std[1], lat_lon_alt_std[2], ang_noise, ang_noise, ang_noise;
    }

    Mat6d V = noise_vec.asDiagonal();
    Eigen::Matrix<double, 18, 6> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + V).inverse();

    // 更新x和cov
    Vec6d innov = Vec6d::Zero();
    innov.template head<3>() = (pose.translation() - p_);          // 平移部分
    //innov.template tail<3>() = Log(rot_) - Log(Rgnss);  // 旋转部分(3.67)
    innov.template tail<3>() = Log((rot_.inverse() * Rgnss));  // 旋转部分(3.67)
    
    dx_ = K * innov;
    cov_ = (Mat18T::Identity() - K * H) * cov_;

    UpdateAndReset();

    return true;
}

bool QuaternionKinematics::ObserveWheelSpeed(const Odom& odom) {
    assert(odom.timestamp_ >= current_time_);

    options_.odom_span_ = odom.timestamp_ - last_odom_.timestamp_;
    if(options_.odom_span_ > 1.0 || options_.odom_span_ < 1.0e-5) {
        options_.odom_span_ = 0.01;
    }     
    double velo_l = options_.left_wheel_radius_ * (odom.left_pulse_) / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
    double velo_r = options_.right_wheel_radius_ * (odom.right_pulse_) / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
  
    double average_vel = 0.5 * (velo_l + velo_r);

    velocity_ = average_vel;
    
    VecT vel_odom(average_vel, 0.0, 0.0);
    
    if(average_vel < 0.00000001) {
        double o2 = 1.0/(options_.odom_var_ * options_.odom_var_);
        odom_noise_.diagonal() << o2, o2, o2;
        //LOG(INFO) << "odom_noise_ = " << odom_noise_;
    } else {
        double o2 = (options_.odom_var_ * options_.odom_var_);
        odom_noise_.diagonal() << o2, o2, o2;
    }

    if(!is_use_wheel_se3_) {
        // odom 修正以及雅可比
        // 使用三维的轮速观测，H为3x18，大部分为零
        Eigen::Matrix<double, 3, 18> H = Eigen::Matrix<double, 3, 18>::Zero();
        H.template block<3, 3>(0, 3) = Mat3T::Identity();
        // 卡尔曼增益
        Eigen::Matrix<double, 18, 3> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + odom_noise_).inverse();
        VecT vel_world = rot_ * vel_odom;
        dx_ = K * (vel_world - v_);
        // update cov
        cov_ = (Mat18T::Identity() - K * H) * cov_;
        UpdateAndReset();        
    }


    // Pre-computation.
    const double left_dist = odom.left_pulse_ * 0.00047820240382508;
    const double right_dist = odom.right_pulse_ * 0.00047768621928995;
    const double delta_yaw = (right_dist - left_dist) / 1.52439;
    const double delta_dist = (right_dist + left_dist) * 0.5;
    // Mean.
    const Eigen::Matrix3d delta_rot = Eigen::AngleAxisd(delta_yaw, Eigen::Vector3d::UnitZ()).toRotationMatrix();
    const Eigen::Vector3d delta_trans = Eigen::Vector3d(delta_dist, 0.0, 0.0);

    //VecT wheel_trans = wheel_trans_ + wheel_rot_ * delta_trans;
    //Mat3T wheel_rot = wheel_rot_ * delta_rot;
    //wheel_trans_ = p_; 
    //wheel_rot_ = rot_;

    wheel_trans_ = wheel_trans_ + wheel_rot_ * delta_trans;
    wheel_rot_ = wheel_rot_ * delta_rot;

    //LOG(INFO) << "wheel_trans_ = " << wheel_trans_.transpose();

    double trans_noise = 0.5;
    double ang_noise = 1.0 * math::kDEG2RAD;
    ObserveSE3(SE3(wheel_rot_, wheel_trans_), trans_noise, ang_noise);
    //ObserveRotation(SE3(wheel_rot_, wheel_trans_), trans_noise, ang_noise);

    //LOG(INFO) << "GetNominalSE3() = " << GetNominalSE3().translation().transpose();

    last_odom_ = odom;
    return true;
}

void QuaternionKinematics::UpdateAndReset() {
    Eigen::Matrix3d delta_rot = Eigen::Matrix3d::Identity();
    if (dx_.template block<3, 1>(6, 0).norm() > 1e-15) {
        delta_rot = Eigen::AngleAxisd(dx_.template block<3, 1>(6, 0).norm(), dx_.template block<3, 1>(6, 0).normalized()).toRotationMatrix();
    }

    p_ += dx_.template block<3, 1>(0, 0);
    v_ += dx_.template block<3, 1>(3, 0);
    rot_ = rot_ * delta_rot;

    //rot_ = rot_ * Eigen::AngleAxisd(dx_.template block<3, 1>(6, 0).norm(), dx_.template block<3, 1>(6, 0).normalized()).toRotationMatrix();

    if (options_.update_bias_gyro_) {
        bg_ += dx_.template block<3, 1>(9, 0);
    }

    if (options_.update_bias_acce_) {
        ba_ += dx_.template block<3, 1>(12, 0);
    }

    g_ += dx_.template block<3, 1>(15, 0);

    //LOG(INFO) << "dx_ = " << dx_.transpose();

    ProjectCov();
    dx_.setZero();
}

void QuaternionKinematics::UpdateQuaternionAndReset() {
    p_ += dx_.template block<3, 1>(0, 0);
    v_ += dx_.template block<3, 1>(3, 0);
    //rot_ = rot_ * Eigen::AngleAxisd(dx_.template block<3, 1>(6, 0).norm(), dx_.template block<3, 1>(6, 0).normalized()).toRotationMatrix();
    if (dx_.block<3, 1>(6, 0).norm() > 1e-12) {
        Eigen::Vector3d delta_q = dx_.block<3, 1>(6, 0);
        Eigen::Quaterniond temp_quat;

        temp_quat.w() = 1.0;
        temp_quat.x() = delta_q[0]/2;
        temp_quat.y() = delta_q[1]/2;
        temp_quat.z() = delta_q[2]/2;
        quat_ *= temp_quat;

        //LOG(INFO) << "state_.rot = " << state_.rot;
    }

    quat_.normalized();
    rot_ = quat_.toRotationMatrix();

    if (options_.update_bias_gyro_) {
        bg_ += dx_.template block<3, 1>(9, 0);
    }

    if (options_.update_bias_acce_) {
        ba_ += dx_.template block<3, 1>(12, 0);
    }

    g_ += dx_.template block<3, 1>(15, 0);

    LOG(INFO) << "dx_ = " << dx_.transpose();

    ProjectCov();
    dx_.setZero();
}

void QuaternionKinematics::ProjectCov() {
    Mat18T J = Mat18T::Identity();
    J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SkewMatrix(dx_.template block<3, 1>(6, 0));
    cov_ = J * cov_ * J.transpose();
}

Eigen::Matrix3d QuaternionKinematics::ComputeRotationJacbian() {
    Eigen::Matrix3d right_jac_inv = Eigen::Matrix3d::Identity();
    Eigen::Vector3d axis_angle = Log(rot_);
    double theta_norm = axis_angle.norm();
    double part_one = 1.0/theta_norm/theta_norm;
    double part_two = -(1+cos(theta_norm))/(2.0*theta_norm*sin(theta_norm));
    Eigen::Matrix3d theta_pow = SkewMatrix(axis_angle)*SkewMatrix(axis_angle);
    right_jac_inv = Eigen::Matrix3d::Identity()+0.5*SkewMatrix(axis_angle)+
                    (part_one + part_two)*theta_pow;
    //LOG(INFO) << "rot_ = " << rot_;
    //LOG(INFO) << "theta_norm = " << theta_norm;
    //LOG(INFO) << "right_jac_inv = " << right_jac_inv;
    return right_jac_inv;
}

Eigen::Matrix3d QuaternionKinematics::Exp(const Eigen::Vector3d &axis_angle) {
    double norm = axis_angle.norm();
    if (norm > 1e-12) {
        Eigen::Vector3d r = axis_angle / norm; 
        Eigen::Matrix3d skew_axis = SkewMatrix(r); 
        double theta = norm; 
        return Eigen::Matrix3d::Identity() + std::sin(theta) * skew_axis + (1.0 - std::cos(theta)) * skew_axis * skew_axis;
    }

    return Eigen::Matrix3d::Identity();
}

Eigen::Vector3d QuaternionKinematics::Log(const Eigen::Matrix3d& R) {
    /*Eigen::Vector3d axis_angle;
    double trace = R(0,0)+R(1,1)+R(2,2);
    double x = (R.trace()-1.0)/2.0;
    if(x > 1.0) {
        x = 1.0;
    } 
    if(x < -1.0) {
        x = -1.0;
    }

    double angle = acos(x);

    Eigen::Matrix3d rot_minus_rot_trans = R - R.transpose();
    double wx = (rot_minus_rot_trans(2,1) - rot_minus_rot_trans(1,2))/2.0;
    double wy = (rot_minus_rot_trans(0,2) - rot_minus_rot_trans(2,0))/2.0;
    double wz = (rot_minus_rot_trans(1,0) - rot_minus_rot_trans(0,1))/2.0;

    Eigen::Vector3d vee_axis(wx,wy,wz);
    axis_angle = vee_axis / (2*sin(angle));*/

    Eigen::AngleAxisd rotation_vector(R);
    double angle = rotation_vector.angle();
    Eigen::Vector3d axis_angle = rotation_vector.axis();
    return angle*axis_angle;
}

Eigen::Matrix<double, 4, 4> QuaternionKinematics::ComputeRightQuaternion(const Eigen::Quaterniond& quat) {
    Eigen::Matrix<double, 4, 4> right_quat;
    right_quat(0,0) = quat.w();
    right_quat(0,1) = -quat.x();
    right_quat(0,2) = -quat.y();
    right_quat(0,3) = -quat.z();
    right_quat(1,0) = quat.x();
    right_quat(1,1) = quat.w();
    right_quat(1,2) = quat.z();
    right_quat(1,3) = -quat.y();

    right_quat(2,0) = quat.y();
    right_quat(2,1) = -quat.z();
    right_quat(2,2) = quat.w();
    right_quat(2,3) = quat.x();
    right_quat(3,0) = quat.z();
    right_quat(3,1) = quat.y();
    right_quat(3,2) = -quat.x();
    right_quat(3,3) = quat.w();
    return right_quat;    
}

}    